/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief Connection dispatcher module implementation.
 * \author Antti Maula <antti.maula@tkk.fi>
 */
#include "owndebug.h"
#include "connectionDisp.h"
#include "hubctrl_utils.h"
#include "buffer.h"
#include "ownutils.h"
#include <pthread.h>
#include <stdlib.h>
#include <assert.h>
#include <unistd.h>
#include <sched.h>
#include <signal.h>
#include <fcntl.h>

//*****************************************************************************
#define CONNECTIONDISP_INTERVAL_TIMER_MS 10000
#define CONNECTIONDISP_MAX_ALLOWED_PACKET_DEFAULT 10000000L
//*****************************************************************************
//*****************************************************************************
static int createServerSockets(TConnectionDisp *cd);
static int createClientSockets(TConnectionDisp *cd);
static int executeProcessingLoop(TConnectionDisp *cd);
static void setIntervalTimer(TConnectionDisp *cd, const unsigned int aInterval_ms);
static void processDisconnectedClientLinks(TConnectionDisp *cd);
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

int connectionDispInit(TConnectionDisp *cd, TConnectionList *cl)
{
  int result = -1;
  
  // Initialize TConnectionDisp memory segment to zero.
  memset(cd, 0x00, sizeof(TConnectionDisp));

  // Assign pointers.
  cd->connectionListPtr = cl;

  // Set IP

  // Verify pointers
  assert(cd->connectionListPtr != NULL);

  // Set default values.
  cd->maxAllowedPacket = CONNECTIONDISP_MAX_ALLOWED_PACKET_DEFAULT;

  // Initialize container mutex.
  pthread_mutex_init(&cd->mutex, NULL);

  // Initialize housekeeping timer.
  setIntervalTimer(cd, CONNECTIONDISP_INTERVAL_TIMER_MS);

  // Init hubctrl_utils (Get Instance pointer from protocol
  // registrant, then assign the instance to hubctrlutils
  const TProtocolEntry *hubctrl_protocolentry = protocolGetProtocolEntry(HUBCTRLPROTOCOL_NUMBER);
  assert(hubctrl_protocolentry != NULL);
  THubCtrl *hubctrl_entry = (THubCtrl *)hubctrl_protocolentry->protocolData;
  hubctrlutils_Init(hubctrl_entry);


  // Add entry
  

  // Set result :)
  result = 0;
 

  return result;
}
//*****************************************************************************

int connectionDispDeinit(TConnectionDisp *cd)
{
  int result = -1;

  // Stop timer
  setIntervalTimer(cd, 0);

  // Deinit msgReplyBuffer
  //msgreplybufferDeinit(&cd->msgReplyBuffer);

  // Last but not least - clear out the mem seg.
  memset(cd, 0x00, sizeof(TConnectionDisp));

  // Clear out mutex
  pthread_mutex_destroy(&cd->mutex);

  return result;
}
//*****************************************************************************

int connectionDispAddServerTCP(TConnectionDisp *cd, int aPort, const char *local_address)
{
  assert(cd != NULL);
  TServerTCP *array = NULL;
  TServerTCP *entry = NULL;

  // Allocate memory for new number of entries.
  cd->servers.serverTCPCount ++;
  array = realloc(cd->servers.tcp, sizeof(TServerTCP) * cd->servers.serverTCPCount );
  assert(array != NULL);
  cd->servers.tcp = array;
  
  // get pointer to the new entry
  entry = &cd->servers.tcp[cd->servers.serverTCPCount-1];
  
  // Assign information (Set to 0x00 and then assign port)
  memset(entry, 0x00, sizeof(TServerTCP));
  entry->port = aPort;
  if (local_address) {
    strncpy(entry->local_address, local_address, sizeof(entry->local_address));
  } else {
    strcpy(entry->local_address,"0.0.0.0");
  }
  
  return 0;
}
//*****************************************************************************

int connectionDispAddClientTCP(TConnectionDisp *cd, const char *aAddr, int aPort)
{  
  assert(cd != NULL);
  TClientTCP *array = NULL;
  TClientTCP *entry = NULL;
  
  // Allocate memory for new number of entries.
  cd->clients.clientTCPCount ++;
  array = realloc(cd->clients.tcp, sizeof(TClientTCP) * cd->clients.clientTCPCount );
  assert(array != NULL);
  cd->clients.tcp = array;
  
  // get pointer to the new entry
  entry = &cd->clients.tcp[cd->clients.clientTCPCount-1];
  
  // Assign information (Set to 0x00 and then assign port and addr)
  memset(entry, 0x00, sizeof(TClientTCP));
  entry->port = aPort;
  strncpy(entry->address, aAddr, sizeof(entry->address));
  entry->address[sizeof(entry->address)-1] = '\0'; // Force terminating NULL.

  return 0;
}
//*****************************************************************************

int connectionDispInitializeSockets(TConnectionDisp *cd)
{
  int result = 0;
  dPrint(ODTEST,"Executing creates...");
  
  // Initially create Server and Client sockets.
  result += createServerSockets(cd);
  result += createClientSockets(cd);

  dPrint(ODTEST,"Creates returned %d", result);

  return result;
}
//*****************************************************************************

int connectionDispExecute(TConnectionDisp *cd)
{
  // execute processing loop.
  const int result = executeProcessingLoop(cd);
  
  return result;
}
//*****************************************************************************

int connectionDispTerminate(TConnectionDisp *cd)
{
  cd->execute = 0;

#warning connectionDispTerminate - Wait for all action to stop.
  return 0;
}
//*****************************************************************************

void connectionDispProcessTimerEvent(TConnectionDisp *cd)
{
  // Currently just wrap the call to process disconnected client
  // links.
  processDisconnectedClientLinks(cd);
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
// Locals for locals.
static int createServerSocketsTCP(TConnectionDisp *cd);
static int createClientSocketsTCP(TConnectionDisp *cd);
static void cleanupConnectionListEntry(TConnectionDisp *cd, TConnectionListEntry *entry);
static void reconnectEntry(TConnectionDisp *cd, TConnectionListEntry *cle);
//*****************************************************************************

static void setIntervalTimer(TConnectionDisp *cd, const unsigned int aInterval_ms)
{
  struct itimerval itv;
  // Construct interval from parameter.
  itv.it_interval.tv_sec = aInterval_ms / 1000;
  itv.it_interval.tv_usec = (aInterval_ms % 1000) * 1000;
  itv.it_value = itv.it_interval;

  int r = setitimer(ITIMER_REAL, &itv, NULL);
  assert(r == 0);
}
//*****************************************************************************

static int createServerSocketsTCP(TConnectionDisp *cd)
{
  int result = 0;
  int i;
  TServerTCP *entry = NULL;
  
  dPrint(ODTEST,"Executing createServerSocketsTCP, serverTCPCount = %d", cd->servers.serverTCPCount);
  
  for(i = 0; i < cd->servers.serverTCPCount; ++i) {
    // Take pointer to entry (contains port and interface to bound to)
    entry = &cd->servers.tcp[i];
    TConnection *c = NULL;
    TConnectionTCP *tcpc = NULL;
    int r;
    
    // Malloc space for new connection.
    c = (TConnection *)malloc(sizeof(TConnection));
    tcpc = (TConnectionTCP *)malloc(sizeof(TConnectionTCP));
    assert(c != NULL);
    assert(tcpc != NULL);
    
    // Init the connection && set settings
    // Initialize TCP connection.
    // Set server, nonblocking, assign shared_socket.
    r =  connectionTCPInit(tcpc);
    assert(r == CONNECTION_OK);
    r |= connectionTCPSetConnectionType(tcpc, tcpStyleListeningSocket, tcpBlockingTypeNonBlocking);
    assert(r == CONNECTION_OK);
    r |= connectionTCPSetPort(tcpc, entry->port);
    assert(r == CONNECTION_OK);
    r |= connectionTCPSetIP(tcpc, entry->local_address);
    assert(r == CONNECTION_OK);

    // Now, give the initialized pointer table to new Connection.
    // (The pointer table includes the 'dataPtr' which points to the
    // constructed TConnectionTCP, and hence is now owned by the
    // TConnection object)
    r |= connectionInit(c, connectionTCPGetConnectionPtrEntry(tcpc));
    if (r != CONNECTION_OK) {
      dPrint(1, "Error setting up new ListeningSocket connection!");
      connectionDestroy(c);
      c = NULL;
      result = -1;

    } else {
      dPrint(2, "Set up a Listening Socket on port %d", entry->port);

      // Call Connect, this will just set up the connection to
      // listening state, as defined by the
      // connectionTCPSetConnectionType call.
      r = connectionConnect(c);
      if (r == 0) {

        // We got a new connection, so add it to connectionList
        TConnectionListEntry *ne = connectionListAddConnection( cd->connectionListPtr,
                                                                KConnectionType_Server,
                                                                KConnectionState_Listening,
                                                                KConnectionProtocol_TCP,
                                                                ( connectionEntryFlag_SocketActive |
                                                                  connectionEntryFlag_SelectSetRead |
                                                                  connectionEntryFlag_SelectSetException),
                                                                c);
        
        if (ne == NULL) {
          // Sorry! We don't have room for this connection. Wait until somebody exits.
          dPrint(2, "Error adding new connection!");
          connectionDestroy(c);
          c = NULL;
          result = -1;
          
        } else {
          // Store callback.
          ne->serverTCPptr = entry;
          
          dPrint(ODSTATUS, "Succesfully set up listening port on '%s:%d'", 
                 connectionGetPeerIP(c), 
                 entry->port);
          
        }
        
      } else { // if (r == 0) {
        dPrint(1, "Connection failed. Abandoning connection.");
        connectionDestroy(c);
        c = NULL;
        result = 1;
      
      }
    }
  }
 

  return result;
}
//*****************************************************************************

static int createClientSocketsTCP(TConnectionDisp *cd)
{
  int result = 0;
  int i;
  TClientTCP *entry = NULL;
  
  dPrint(ODTEST,"Executing createClientSocketsTCP, clientTCPCount = %d", 
         cd->clients.clientTCPCount);
  
  for(i = 0; i < cd->clients.clientTCPCount; ++i) {
    // Take pointer to entry (contains port and interface to bound to)
    entry = &cd->clients.tcp[i];
    TConnection *c = NULL;
    TConnectionTCP *tcpc = NULL;
    int r;
    
    // Malloc space for new connection.
    c = (TConnection *)malloc(sizeof(TConnection));
    tcpc = (TConnectionTCP *)malloc(sizeof(TConnectionTCP));
    assert(c != NULL);
    assert(tcpc != NULL);
    
    // Init the connection && set settings
    // Initialize TCP connection.
    // Set client, nonblocking, assign shared_socket.
    r =  connectionTCPInit(tcpc);
    assert(r == CONNECTION_OK);
    r |= connectionTCPSetConnectionType(tcpc, tcpStyleAsyncClient, tcpBlockingTypeNonBlocking);
    assert(r == CONNECTION_OK);
    printf("Calling with port: %d\n\n", entry->port);
    r |= connectionTCPSetPort(tcpc, entry->port);
    assert(r == CONNECTION_OK);
    r |= connectionTCPSetHost(tcpc, entry->address);
    assert(r == CONNECTION_OK);
    
    // Now, give the initialized pointer table to new Connection.
    // (The pointer table includes the 'dataPtr' which points to the
    // constructed TConnectionTCP, and hence is now owned by the
    // TConnection object)
    r |= connectionInit(c, connectionTCPGetConnectionPtrEntry(tcpc));
    if (r != CONNECTION_OK) {
      dPrint(1, "Error setting up new AsyncClient connection!");
      connectionDestroy(c);
      c = NULL;
      result = -1;
      
    } else {
      dPrint(ODINFO, "Set up a Async Client Socket on '%s:%d'", 
             entry->address, entry->port);
      
      // Call Connect, this will just set up the connection to
      // listening state, as defined by the
      // connectionTCPSetConnectionType call.
      r = connectionConnect(c);
      if (r == 0) {
        // We got a new connection, so add it to connectionList
        TConnectionListEntry *ne = connectionListAddConnection( cd->connectionListPtr,
                                                                KConnectionType_Client,
                                                                KConnectionState_Connecting,
                                                                KConnectionProtocol_TCP,
                                                                ( connectionEntryFlag_SocketActive |
                                                                  connectionEntryFlag_AutoReconnect |
                                                                  connectionEntryFlag_SelectSetWrite |
                                                                  connectionEntryFlag_SelectSetException ),
                                                                c);
                                       
        if (ne == NULL) {
          // Sorry! We don't have room for this connection. Wait until somebody exits.
          dPrint(2, "Error adding new connection!");
          connectionDestroy(c);
          c = NULL;
          result = -1;
          
        } else {
          // Store callback.
          ne->clientTCPptr = entry;
          
          // Construct AutoReconnect parameters.
#warning Using fixed default of 5000ms for reconnect minimum.
          connectionListEntryCreateAutoReconnectCtrl(ne, 5000);


          // Print status
          dPrint(ODSTATUS, "Succesfully set up Asynchronous Client connection to '%s:%d'", 
                 connectionGetPeerIP(c), 
                 entry->port);
          
        }
        
      } else { // if (r == 0) {
        dPrint(1, "Connection failed. Abandoning connection.");
        connectionDestroy(c);
        c = NULL;
        result = 1;
      
      }
    }
  }
 
  return result;
}
//*****************************************************************************

static int createServerSockets(TConnectionDisp *cd)
{
  int result = 0;
  
  // Call TCP Server creator.
  result |= createServerSocketsTCP(cd);

  // Call other server creator.
  // result = createServerSocketsSomeOther(cd);

  return result;
}
//*****************************************************************************

static int createClientSockets(TConnectionDisp *cd)
{
  int result = 0;

  // Call TCP client creator.
  result += createClientSocketsTCP(cd);

  return result;
}
//*****************************************************************************

int processNewServerConnection(TConnectionDisp *cd, TConnectionListEntry *entry)
{
  int result = 0;
  TConnection *c = NULL;

  dPrint(ODTEST,"New server connection coming...");
  
  assert(entry->state == KConnectionState_Listening);

  // Select operation by type (Currently only one type accepted, hence
  // this has only one entry)
  if (entry->type == KConnectionProtocol_TCP) {

    // take pointer to source Entry TConnectionTCP (Needed for shared
    // socket addr)
    assert(entry != NULL);
    assert(entry->connection != NULL);
    assert(entry->connection->link != NULL);
    assert(entry->connection->link->dataPtr != NULL);
    const TConnectionTCP *listenc = (const TConnectionTCP *)(entry->connection->link->dataPtr);
    TConnectionTCP *tcpc = NULL;
    int r;

    // Take connection information pointer.
    TServerTCP *tcps = entry->serverTCPptr;
    assert(tcps != NULL);
    
    // Malloc space for new connection.
    c = (TConnection *)malloc(sizeof(TConnection));
    tcpc = (TConnectionTCP *)malloc(sizeof(TConnectionTCP));
    assert(c != NULL);
    assert(tcpc != NULL);
    
    // Init the connection && set settings
    // Initialize TCP connection.
    // Set server, nonblocking, assign shared_socket.
    r =  connectionTCPInit(tcpc);
    r |= connectionTCPSetConnectionType(tcpc, tcpStyleServer, tcpBlockingTypeNonBlocking);
    r |= connectionTCPSetSocketHandle(tcpc, listenc->socket_fd, &listenc->socket_addr);
    
    // Now, give the initialized pointer table to new Connection.
    // (The pointer table includes the 'dataPtr' which points to the
    // constructed TConnectionTCP, and hence is now owned by the
    // TConnection object)
    r |= connectionInit(c, connectionTCPGetConnectionPtrEntry(tcpc));
    if (r != 0) {
      dPrint(1, "Error setting up new Server connection!");
      connectionDestroy(c);
      c = NULL;
      result = -1;

    } else {
      dPrint(2, "Accepting new connection on port %d", tcps->port);
      int extra_flags = 0;
      
      // Check if authentication is disabled
      if (cd->authMode == 0) {
        extra_flags |= connectionEntryFlag_LinkAuthenticated;
      }

      // Connect! (Wait for single connection)
      r = connectionConnect(c);
      if (r == 0) {
        // We got a new connection, so add it to connectionList
        TConnectionListEntry *ne = connectionListAddConnection( cd->connectionListPtr,
                                                                KConnectionType_Server,
                                                                KConnectionState_Connected,
                                                                KConnectionProtocol_TCP,
                                                                ( connectionEntryFlag_SocketActive |
                                                                  connectionEntryFlag_SelectSetRead |
                                                                  connectionEntryFlag_SelectSetException |
                                                                  extra_flags),
                                                                c);
        
        if (ne == NULL) {
          // Sorry! We don't have room for this connection. Wait until somebody exits.
          dPrint(2, "Error adding new connection!");
          connectionDestroy(c);
          c = NULL;
          result = -1;
          
        } else {
          
          dPrint(1, "Connected to '%s:%d', assigned hubID 0x%08x", 
                 connectionGetPeerIP(c), 
                 connectionGetPeerPort(c),
                 ne->connectionID);
        }
        
      } else { // if (r == 0) {
        dPrint(1, "Connection failed. Abandoning connection.");
        connectionDestroy(c);
        c = NULL;
        result = 1;
      
      }
    }
  }
  return result;
}
//*****************************************************************************

static int processReadDataEvent(TConnectionDisp *cd, TConnectionListEntry *entry)
{
  int result = 0;
  int r;
  int dataSize;
  int packetSize;
  const TProtocolEntry *protocol_entry;
  TProtocolIdentification *protocol_id;
  
  // Trivial checks
  assert(cd != NULL);
  assert(entry != NULL);
  assert(entry->initialized == 1);
  assert(entry->connection != NULL);


  //  dPrint(ODTEST,"Processing ReadData event");

  // Read in data from the activated socket!
  r = connectionListReceiveData( cd->connectionListPtr,
				 entry );

  
  // Check whether the read operation was successfull
  if (r == CONNECTIONLIST_OK) {

    // Verify that the entry really is initialized.
    assert(entry->initialized == 1);
        
    // As long as there is data to process in input queue. 
    int loop = 1;
    //    dPrint(ODTEST,"***** CYCLE BEGIN *****");
    while (loop) {
      
      // Assign locally
      dataSize = bufferGetLength(&entry->inputBuffer);

      // Test that we have enough data for protocol identification, if not. break.
      if (dataSize < sizeof(TProtocolIdentification))
        break; // while(loop)

      // OK. We know now that we have enough data for protocol
      // identification, so DO IT!
      protocol_id = (TProtocolIdentification *)bufferGetPtr(&entry->inputBuffer);
      
      // Check whether the ProtocolID is 'sane'
      packetSize = protocol_id->total_size;
      if (packetSize <= cd->maxAllowedPacket && 
          packetSize >= sizeof(TProtocolIdentification)) {

        // Now, test whether the whole packet has arrived, if not;
        // break.
        if (dataSize < packetSize) 
          break; // while(loop)
        
        // And we have data for the whole next data
        // packet. Determining type:
        protocol_entry = protocolGetProtocolEntry(protocol_id->protocol_number); 
        if (protocol_entry == NULL) {
          dPrint(ODTEST,"Packet containing unsupported protocol '0x%02x' received! Closing link!",
                 protocol_id->protocol_number);
          result = (1<<0);
          break; // while(loop)
        }
        
        // Data was read OK, so pass the handle to protocol handler,
        // which will read the data directly Call the current
        // protocolhandler through it's pointer
        /*dPrint(ODTEST,"**** Calling protocolHandler; dataInBuffer = %dbytes, packetSize = %dbytes, cd->connectionListPtr = %p, entry = %p, entry->connectionID = %08x",
               dataSize, 
               packetSize,
               cd->connectionListPtr, 
               entry, 
               entry->connectionID);*/
        r = protocol_entry->protocolHandler(protocol_entry->protocolData,
                                            cd->connectionListPtr,
                                            entry);
        
        // Act upon the return value
        switch(r) {
        case protocolExitNoError:
          // Shift the buffer, the handler accepted data.
          assert(packetSize >= 0);
          bufferShift(&entry->inputBuffer, packetSize);
          //          dPrint(ODTEST, "NoError return, shifted %u bytes out, buffer size now %u bytes", packetSize, bufferGetLength(&entry->inputBuffer));
          break;
          
        case protocolExitError:
          dPrint(ODTEST, "Protocol handler exited with Error! Doing error handling!");
          bufferClear(&entry->inputBuffer);
          loop = 0;
          result = (1<<0);
          break;
          
        default:
          dPrint(ODTEST,"WARNING: Protocol handler returned unknown value %d!",
                 r);
          break;
        }
      } else {
        dPrint(ODTEST,"WARNING: Excessive/Invalid frame sent by client 0x%08x! Closing link!",
               entry->connectionID);
        loop = 0;
        result = (1<<0);
	break;
      }
    }
  //    dPrint(ODTEST,"***** CYCLE END *****");
  } else {
    // It wasn't, throw a message and call error handler for the connection 
    dPrint(ODTEST,"Failed to receive data from entry 0x%p (0x%08x)!", entry, entry->connectionID);
    //connectionDispErrorHandler(cd, e, poll_entry);
    result = (1<<0);

  }

  return result;
}
//*****************************************************************************

static int processReadEvent(TConnectionDisp *cd, TConnectionListEntry *entry)
{
  int result = 0;

  //  dPrint(ODTEST,"Read event, entry %p, ID 0x%08x", entry, entry->connectionID);
  
  // Take local variable for flags. Easier and avoids pointer dereference.
  const unsigned int flags = entry->flags;

  // First of all, is this socket active?
  if ( flags & connectionEntryFlag_SocketActive ) { 

    // Socket is active, proceed. Check flags.
    if (entry->state == KConnectionState_Listening) {
      
      // Socket is a listening socket -> process new server connection.
      result = processNewServerConnection(cd, entry);
      
    } else if (entry->state == KConnectionState_Connected) {
      
      // Connected socket and this is Read event. So process it with
      // reader.
      result = processReadDataEvent(cd, entry);
      
    } else {
      // Not listening nor connected, hence do nothing with it.
      // This branch intentionally left empty :)
    }
  }

  return result;
}
//*****************************************************************************

static int processWriteEvent(TConnectionDisp *cd, TConnectionListEntry *entry)
{
  int result = 0;  
  
  //  dPrint(ODTEST,"Write event!");
  // Take local variable for flags. Easier and avoids pointer dereference.
  const unsigned int flags = entry->flags;
  
  // Just verify
  assert(flags & connectionEntryFlag_SocketActive);
  
  // Is the WRITE activated socket a DATASOCKET?
  if (entry->state == KConnectionState_Connected &&
      flags & connectionEntryFlag_DataInOutputBuffer ) {
    // Yes, indeed. We have an 'clear to write' for an datasocket.  We
    // got WRITE permission for entry, so attempt to flush output
    // buffer.
    int r = connectionListFlushOutputBuffer(cd->connectionListPtr, entry);
    
    if (r == CONNECTIONLIST_OK) {
      //      dPrint(ODTEST,"Flushing outputbuffer OK, clearing SelectSetWrite bit");
      
      // Now,
      connectionListUpdateEntryFlags(cd->connectionListPtr, 
                                     entry, 
                                     flags & ~(connectionEntryFlag_SelectSetWrite|
                                               connectionEntryFlag_DataInOutputBuffer));

    } else {
      //dPrint(ODTEST,"OutputBuffer not emptied, hence not clearing the SelectSetWrite bit");
      
    }
  
  } else if (entry->state == KConnectionState_Connecting) {
    // Socket in Connecting state (not yet verified to be connected)
    int value;
    unsigned int vsize = 4;
    if (getsockopt(connectionGetHandle(entry->connection),
                   SOL_SOCKET,
                   SO_ERROR,
                   &value,
                   &vsize) == 0) {
      
      dPrint(ODSTATUS, "Succesfully connected to target '%s:%d', local connectionID is %08x", 
             connectionGetPeerIP(entry->connection), 
             entry->clientTCPptr->port,
             entry->connectionID);

      // Now, remove SelectSetWrite and ConnectingSocket flags, add
      // DataSocket flag and SelectSetRead.
      entry->state = KConnectionState_Connected;
      connectionListUpdateEntryFlags(cd->connectionListPtr, 
                                     entry, 
                                     (flags & ~connectionEntryFlag_SelectSetWrite) |
                                     connectionEntryFlag_SelectSetRead);

      // Last but not least, send registration request to connected HUB.
      const uint32_t lid = connectionListGetLocalHubID(cd->connectionListPtr);
      
      dPrint(ODTEST,"Attempting to register ourself as AP %08x...", lid);

      hubctrlutils_RequestRegisterAP(cd->connectionListPtr,
                                     entry,
                                     lid);

    } else {
      dPrint(ODTEST,"Connection to Client '%s:%d' failed! Will someday try again :)");
      result = (1<<1);
    }
               
    
  } else {
    dPrint(ODTEST,"WTF?");

  }

  return result;
}
//*****************************************************************************

static int processExceptionEvent(TConnectionDisp *cd, TConnectionListEntry *entry)
{
  int result = 1;
  dPrint(ODTEST,"Exception event!");
  return result;
}
//*****************************************************************************

static void processErrorEvent(TConnectionDisp *cd, TConnectionListEntry *entry)
{
  // Do some rather trivial tests
  assert(cd != NULL);
  assert(entry != NULL);

  //  dPrint(ODTEST,"Called!");
  
  const unsigned int flags = entry->flags;
  
  // Was entry previously connected?
  if (entry->state == KConnectionState_Connected) {

    // Is entry client?
    if (entry->type == KConnectionType_Client) {
      dPrint(ODTEST,"Error on Client socket, checking for autoconnection and updating flags as required.");
      
      // Set state to Disconnected.
      entry->state = KConnectionState_Disconnected;

      // Increment unconnectedClientSocketsCount.
      cd->disconnectedClientSocketsCount++;
      
      // Check for AutoReconnect flag.
      if (flags & connectionEntryFlag_AutoReconnect) {
        dPrint(ODTEST,"AutoReconnect is enabled for Client Connection %p, scheduling for AutoReconnect.");

        // Failed entry was a ClientSocket with AutoReconnect!
        connectionListUpdateEntryFlags(cd->connectionListPtr, 
                                       entry, 
                                       entry->flags &
                                       ~(connectionEntryFlag_SelectSetRead));
        
      } else {
        dPrint(ODTEST,"No AutoReconnect set. Cleaning up Client connection");

        // Call cleanup
        cleanupConnectionListEntry(cd, entry);

      }

    } else if (entry->type == KConnectionType_Server) {
      // Failed entry was a server socket, thus normal cleanup
      // procedure applies.

      cleanupConnectionListEntry(cd, entry);
      
    } else {
      //dPrint(ODTEST,"entry->state = %d, entry->type = %d",
      //       entry->state, entry->type);

    }
  } else {
    dPrint(ODTEST,"Entry not in Connected state.");

  }
  
  // Return
  return;
}
//*****************************************************************************

static void processIdleEvent(TConnectionDisp *cd)
{
  const unsigned int trigger_idle_value = 1 * 60 * 1000;
  static ownTime_ms_t update = 0;
  //  dPrint(ODTEST,"Idle...");


  if (ownTime_get_ms_since(update) > trigger_idle_value) {
    dPrint(ODVINFO, "-- MARK --");
    update = ownTime_get_ms();
  }
}
//*****************************************************************************

static void processDisconnectedClientLinks(TConnectionDisp *cd)
{ 
  TIDMapEntryIterator ei;
  TConnectionListEntry *entry;

  //  dPrint(ODTEST,"Processing diconnected entries..");
  
  // Check whether there is unconnected entries.events.
  pthread_mutex_lock(&cd->mutex);
  //  dPrint(ODTEST,"Entered mutex.");
  if (cd->disconnectedClientSocketsCount) {
    // Get iterator to beginning of connection list.
    connectionListGetConnectionIterator(cd->connectionListPtr, &ei);
    
    // Get connections all in order and check the connection for events.
    while( (entry = connectionListGetNextConnection(cd->connectionListPtr, &ei)) ) {
      if (entry->state == KConnectionState_Disconnected) {
      

        // plus, currently not DataSocket (not open)
        //        dPrint(ODTEST,"Found an entry matching criteria.");
        
        // Calling reconnectEntry.
        reconnectEntry(cd, entry);
      }
    }
  }
  //  dPrint(ODTEST,"Exited mutex");
  pthread_mutex_unlock(&cd->mutex);
}
//*****************************************************************************

static void reconnectEntry(TConnectionDisp *cd, TConnectionListEntry *cle)
{
  assert(cle->state == KConnectionState_Disconnected);
  
  // Check for autoreconnect parameters
  if (cle->autoReconnectCtrlPtr) {
    // Update stats.
    TAutoReconnectCtrl *arc = cle->autoReconnectCtrlPtr;
    
    // Check that the reconnect is not too soon.
    if (ownTime_get_ms_since( arc->last_reconnect_timestamp ) > 
        arc->reconnect_min_interval ) {
      // Update attempt counter.
      arc->reconnect_attempt_count ++;

      // Update last reconnect timestamp.
      arc->last_reconnect_timestamp = ownTime_get_ms();

      // Print status
      dPrint(ODTEST,"Attempting to reconnect Link %p, this is reconnect attempt number %u",
             cle, arc->reconnect_attempt_count);
      
      // Proceed with connection reconnect processing.
      cle->state = KConnectionState_Connecting; 
      connectionListUpdateEntryFlags(cd->connectionListPtr, 
                                     cle, 
                                     (cle->flags |
                                      connectionEntryFlag_SelectSetWrite |
                                      connectionEntryFlag_SelectSetException) &
                                     ~connectionEntryFlag_SelectSetRead);
      
      // Command new connect! (Async style should handle the rest);
      connectionConnect(cle->connection); // Just do it.
      
    } else {
      dPrint(ODTEST,"Minimum of %ums left until new reconnect attempt.");
    }
    
  } else {
    dPrint(ODTEST,"No autoReconnect parameters available, will not autoreconnect.");
  }
}
//*****************************************************************************

static void cleanupConnectionListEntry(TConnectionDisp *cd, TConnectionListEntry *entry)
{
  int i;
  const TProtocolEntry *pentry;
  
  dPrint(ODTEST,"Calling cleanup for ServerSocket with connectionID '0x%08x'",
         entry->connectionID);
  
  // Call error handlers for all registered protocols
  for(i=0; i < MAX_PROTOCOL_COUNT; ++i) {
    pentry = protocolGetProtocolEntry(i);
    if (pentry != NULL && pentry->protocolErrorHandler != NULL) {
      //      dPrint(ODTEST, "Calling error handler for protocol '%02x'",
      //       pentry->protocol_number);
      
      pentry->protocolErrorHandler(pentry->protocolData, 
                                   cd->connectionListPtr,
                                   entry);
    }
  }
  
  // Handlers called. Now take care of the entry.
  dPrint(ODTEST,"Calling cleanup from connectionList for ID '0x%08x'",
         entry->connectionID);
  connectionListRemoveConnection(cd->connectionListPtr, entry->connectionID);
  
  dPrint(ODTEST,"Calling cleanup on entry.");
  connectionListEntryDestroy(entry);

  dPrint(ODTEST,"Cleanup on entry %p complete.", entry);
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*** Conditional block; defining processing function for select() mechanism **
//*****************************************************************************
//*****************************************************************************
#if defined(USE_SELECT_IO) 

static int executeProcessingLoop(TConnectionDisp *cd)
{
  int result = 0;
  fd_set read_set;              // select() Read set
  fd_set write_set;             // select() Write set
  fd_set exception_set;         // select() Exception set
  int select_timeout_ms = 1000; // Timeout for select.

  // Zero sets.
  FD_ZERO(&read_set);
  FD_ZERO(&write_set);
  FD_ZERO(&exception_set);
  
  // Checks
  assert(cd->execute == 0);
  assert(cd->connectionListPtr != NULL);


  // assign.
  cd->execute = 1;
  
  // Test and run.
  int r;
  while(cd->execute) {
    // #warning "Don't forget this Sleep here :)"
    //    usleep(1000*500);

    pthread_mutex_lock(&cd->mutex);
    // Call the connectionList 'select()' wrapper.
    r = connectionListDoSelect(cd->connectionListPtr,
                               &read_set,
                               &write_set,
                               &exception_set, 
                               select_timeout_ms);
    if (r > 0) {
      // Got events!
      //      dPrint(ODTEST,"Got %d events.", r);
      TIDMapEntryIterator ei;
      TConnectionListEntry *entry;

#warning Is this correct?
      // Get iterator to beginning of connection list.
      connectionListGetConnectionIterator(cd->connectionListPtr, &ei);

      // Get connections all in order and check the connection for events.
      while( (entry = connectionListGetNextConnection(cd->connectionListPtr, &ei)) ) {
        // FIRST; Check for immediate flags.
        if (entry->flags & connectionEntryFlag_SkipOnce) {
          entry->flags ^= connectionEntryFlag_SkipOnce;
          continue;
        }
        
        // Take values to local variables for fast access.
        int opr = 0;
        const int fd = connectionGetHandle(entry->connection);

        //        dPrint(ODTEST,"Checking entry %p for events, fd == %d",  entry, fd);

        /////////////////////////////////////////////////////////////
        // Do checks for selection status values. The order of these
        // is carefully thought. First; processing any outstanding
        // errors. Then, processing all WRITE operations to clear out
        // output buffers if such exists. After that, process READ
        // entries (which may again fill in the outputBuffer)
        /////////////////////////////////////////////////////////////
   
        // On EXCEPTION set.
        if (FD_ISSET(fd, &exception_set)) {
          opr |= processExceptionEvent(cd, entry);
        }
        
        // On WRITE set.
        if (!opr && FD_ISSET(fd, &write_set)) {
          opr |= processWriteEvent(cd, entry);
        }
        
        // on READ set.
        if (!opr && FD_ISSET(fd, &read_set)) {
          //          dPrint(ODTEST,"fd = %d found from READ set, calling processReadDataEvent", fd);
          opr |= processReadEvent(cd, entry);
        }
        
        // Check result of processing, if something went wrong,
        // process error.
        if (opr) {
          // opr was not zero, so some processing required.
          processErrorEvent(cd, entry);
        }

      }

      // Processing entries done.
      
    } else {
      // No events to process. So call processIdleEvent.
      processIdleEvent(cd);

    }
    pthread_mutex_unlock(&cd->mutex);
    ownSched_Yield();
  }
  
  return result;
}

//*****************************************************************************
//*****************************************************************************
//*** Conditional block; defining processing function for epoll() mechanism ***
//*****************************************************************************
//*****************************************************************************
#elif defined(USE_EPOLL_IO)

static int executeProcessingLoop(TConnectionDisp *cd)
{
  int result = 0;
  int epoll_timeout_ms = 1000;

  // Define 10 elements
  struct epoll_event epoll_event_array[10];
  const int max_epoll_events = 1;

  // Checks
  assert(cd->execute == 0);
  assert(cd->connectionListPtr != NULL);

  // assign.
  cd->execute = 1;
  
  // Test and run.
  int r;
  while(cd->execute) {
#warning "Don't forget this Sleep here :)"
    //    usleep(1000*500);

    pthread_mutex_lock(&cd->mutex); 
    // Call the connectionList 'select()' wrapper.
    r = connectionListDoEpollWait(cd->connectionListPtr,
                                  epoll_event_array,
                                  max_epoll_events,
                                  epoll_timeout_ms);
    if (r > 0) {
      // Got events!
      //      dPrint(ODTEST,"Got %d events.", r);
      int i;
      for(i=0; i < r; ++i) {
        TConnectionListEntry *cle = epoll_event_array[i].data.ptr;
        assert(cle != NULL);

        // Entry 'cle' has event.
#warning Process the event
      }

      // Processing entries done.
      
    } else {
      // No events to process. So call processIdleEvent.
      processIdleEvent(cd);

    }
    pthread_mutex_unlock(&cd->mutex);
  }
  
  return result;
}

#endif
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
